/*
frigOS
Licensed under GPLv3
*/

#include "serial.h"
#include "adc.h"
#include "ax12.h"
#include "axs1.h"
#include "cm5.h"
#include "led.h"
#include "protocol.h"
#include "kernel.h"
#include "shared_memory.h"

#include <avr/interrupt.h>
#include <util/delay.h>


uint8_t reply_status;
uint8_t reply_valid;

uint8_t reply[4];
int servoIdx;

void initRS485(void)
{
    // set data direction output
    // RS485 half duplex
    DDRE |= (1 << DDE3) | (1 << DDE2);

} // initRS485


/**********************************************************************************/
/** ENQUEUE/DEQUEUE FUNCTIONS *****************************************************/
/**********************************************************************************/

// add a byte to the end of the queue
void enqueue(uint8_t data, volatile IO_Queue *queue)
{
    if(!queue->full)
    {
        queue->buffer[queue->in] = data;
        queue->in = (queue->in + 1) % queue->maxLength;
        queue->queueLength++;
        queue->empty = FALSE;

        // check if the queue is full
        if (((queue->in+1) % queue->maxLength) == queue->out)
        {
            queue->full = TRUE;
        }
    }

    // dequeue a byte and transmit it if necessary
    if (queue->txOn != TX_ON_NONE && queue->txOnEnqueue && !queue->empty)
    {
        switch(queue->txOn)
        {
        case TX_ON_USART0:
            data = dequeue(queue);

            // send the byte
            queue->txOnEnqueue= FALSE;

            // wait for the register to be empty
            while ( !( UCSR0A & (1 << UDRE0) ) )
                asm volatile("nop"::);
            UDR0 = data;

            break;

        case TX_ON_USART1:
            txdLEDOn();

            data = dequeue(queue);

            queue->txOnEnqueue = FALSE;

            // wait for the register to be empty
            while ( !( UCSR1A & (1 << UDRE1) ) )
                asm volatile("nop"::);
            UDR1 = data;

            txdLEDOff();

            break;

        default:
            break;
        }
    }
}

// take a byte off the front of the queue and returns it
// will return 0x00 if the queue is empty, so check queue->isEmpty before
// attempting to dequeue
uint8_t dequeue(volatile IO_Queue *queue)
{
    if(queue->empty)
        return 0x00;    // we don't care; the caller should check if the queue is empty before calling dequeue

    uint8_t data = queue->buffer[queue->out];
    queue->out = (queue->out+1) % queue->maxLength;
    queue->queueLength--;
    queue->full = FALSE;

    if(queue->in == queue->out)
        queue->empty = TRUE;

    return data;
}

char nibble2char(uint8_t nibble)
{
    nibble = ((nibble << 4) >> 4) & 0x0f;
    if(nibble<10)
        return '0'+nibble;
    else
        return 'a'+nibble-10;
}

// convert a byte into a 2-byte hex string, stored in dest
void byte2hex(uint8_t src, char dest[])
{
    uint8_t lo4 = src & 0x0f;
    uint8_t hi4 = (src & 0xf0) >> 4;

    dest[0] = nibble2char(hi4);
    dest[1] = nibble2char(lo4);
}
